//
// Created by haoy on 2017/6/23.
//
#include <string.h>

#include "rtp/rtp_red.hpp"

int RtpRed::read(const uint8_t *data, int size, const RtpMap *rtp_map) {
    redundant_blocks.clear();
    if (size < 4) {
        return 0;
    }
    uint8_t more = 1;
    uint16_t blk_hdr = 0;
    while (more == 1 && size >= blk_hdr + 1) {
        more = data[blk_hdr] >> 7;
        uint8_t pt = data[blk_hdr] & 0x7F;
        int codec = rtp_map->codec(pt);
        uint16_t ts_offset = 0;
        uint16_t blk_len = size - blk_hdr - 1;
        if (more == 1) {
            if (size < blk_hdr + 4) {
                return 0;
            }
            ts_offset = data[blk_hdr + 2];
            ts_offset <<= 6;
            ts_offset |= data[blk_hdr + 3] >> 2;
            blk_len = data[blk_hdr + 3] & 0x03;
            blk_len <<= 8;
            blk_len |= data[blk_hdr + 4];
            redundant_blocks.emplace_back(Block{codec, nullptr, blk_len, ts_offset});
            blk_hdr += 4;
        } else {
            primary_block = Block{codec, nullptr, blk_len, ts_offset};
            blk_hdr += 1;
        }
    }
    uint16_t blk_bdy = blk_hdr;
    for (auto& b : redundant_blocks) {
        if (blk_bdy + b.payload_size > (uint32_t)size) {
            return 0;
        }
        b.payload = const_cast<uint8_t*>(data + blk_bdy);
        blk_bdy += b.payload_size;
    }
    if (blk_bdy + primary_block.payload_size > (uint32_t)size) {
        return 0;
    }
    primary_block.payload = const_cast<uint8_t *>(data + blk_bdy);
    blk_bdy += primary_block.payload_size;
    return blk_bdy;
}

int RtpRed::write(uint8_t *data, int size, const RtpMap *rtp_map) const {
    if (size <= 0 
    || (uint32_t)size < 4 * redundant_blocks.size() + 1) {
        return 0;
    }
    uint16_t blk_hdr = 0;
    uint16_t blks_size = 0;
    for (auto& b : redundant_blocks) {
        data[blk_hdr] = 0x80 | rtp_map->pt(b.codec);
        data[blk_hdr + 1] = b.ts_offset >> 6 & 0xFF;
        data[blk_hdr + 2] = ((b.ts_offset & 0x3F) << 2) | (b.payload_size >> 8 & 0x03);
        data[blk_hdr + 3] = (b.payload_size & 0xFF);
        blk_hdr += 4;
        blks_size += b.payload_size;
    }
    data[blk_hdr] = rtp_map->pt(primary_block.codec) & 0x7F;
    blk_hdr += 1;
    blks_size += primary_block.payload_size;
    if (size < blk_hdr + blks_size) {
        return 0;
    }
    uint16_t blk_bdy = blk_hdr;
    for (auto& b : redundant_blocks) {
        memcpy(data + blk_bdy, b.payload, b.payload_size);
        blk_bdy += b.payload_size;
    }
    memcpy(data + blk_bdy, primary_block.payload, primary_block.payload_size);
    blk_bdy += primary_block.payload_size;
    return blk_bdy;
}
